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Sistema para estimar posicion, velocidad y orientaci6n de un vehfculo (10), que 
comprende medios para determinar las componentes, segun ejes cuerpo del vehfculo, de 
dos vectores unitarios constantes no colineales g b ?e b \ medios para determinar las 
componentes, segdn ejes tierra, de dichos vectores unitarios constantes no colineales 
g n e t ; medios para determinar las tres componentes de velocidad angular 3 b del vehfculo 
en ejes cuerpo; medios para corregir dicha velocidad angular 3 b con una correccion w ffl y 
i obtener una velocidad angular corregida a> b = 65 b +u Q} ; un modulo de control (14), que 
| implementa una ley de control para calcular dicha correcci6n , donde dicha ley de 
! control es: 

u„=o-(g b xg b + e b xe b ) [1] 

; siendo a un escalar positive 

i 

j de forma que al utilizar dicha velocidad angular corregida m * = ^ +w «> como entrada a un 
j modulo de integracion de las ecuaciones cinematicas, estas son estables en sentido ISS y 

el error en la estimacion de la matriz de cosenos directores B y de los angulos de Euler 

: Q> se mantiene acotado. 
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(57) RESUMEN 

SISTEMA Y METODO DE FUSION DE SENSORES PARA ESTIMAR POSICION, VELOCIDAD Y ORIENTACION 
DE UN VEHICULO, ESPECIALMENTE UNA AERONAVE. 

Sistema para estimar posicion, velocidad y orientacion de un vehiculo (10), que 
comprende medios para determinar las componentes, segun ejes cuerpo del vehiculo, de 
dos vectores unitarios constantes no colineales g b ,e b ; medios para determinar las 
componentes, segun ejes tierra, de dichos vectores unitarios constantes no colineales 
g \ A ■ medios para determinar las tres componentes de velocidad angular 3 b del vehiculo 
en ejes cuerpo; medios para corregir dicha velocidad angular 3 b con una correccion u a y 
obtener una velocidad angular corregida ^ = <S b + « OJ ; un modulo de control (14), que 
implementa una ley de control para calcular dicha correccion u m , donde dicha ley de 

, laa . \ u a = o-(g b xg b + e b xe b ) [1] 

control es: j 

siendo <r un escalar positivo, 

de forma que al utilizar dicha velocidad angular corregida = *> +u °> como entrada a un 
modulo de integration de las ecuaciones cinematicas, estas son estables en sentido ISS y 
el error en la estimation de la matriz de cosenos directores B y de los angulos de Euler 
<t> se mantiene acotado. 
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S1STEMA Y METODO DE FUSION DE SENSORES PARA ESTIMAR POSICION. 
VELOCIDAD Y ORIENTACION DE UN VEHICULO. ESPECIALMENTE UNA 
AERONAVE 

Campo de la invencion 
El presente invento se refiere a un metodo y sistema para integrar datos 
procedentes de diversos sensores y estimar la posicion, velocidad y orientacion de un 
vehfculo. Es especialmente apropiada su aplicacion a Aeronaves No Tripuladas (UAV) 
que incorporan sensores de bajo coste para: 

- determinar la actitud y orientacion del vehfculo; 

- determinar su posicion y velocidad; 

- navegar durante periodos de tiempo limitados sin GPS. 

La invencion se Integra dentro de los campos de fusion de sensores, 
determinacion de actitud y navegacion inercial. 

Antecedentes de la invencion 

Existen diversos metodos para estimar la posicion, velocidad y orientacion de 
un vehfculo: 

1.- Sistema de navegacion inercial o INS: Integra las aceleraciones y 
velocidades angulares proporcionadas por una Unidad de Medida Inercial (IMU) para 
calcular la posicion, velocidad y orientacion del vehfculo. Como esta integracion es 
neutralmente estable, los errores se acumulan y las soluciones obtenidas divergen 
rapidamente, salvo que se utiiicen sensores de muy alta calidad, que elevan el coste y 
peso del sistema. 

La solucion pasa por estabilizar la integracion mediante realimentacion de 
bucle cerrado con medidas de otros sensores que no deriven con el tiempo. Con este 
proposito, se utilizan medidas de velocidad aerodinamica y las proporcionadas por 
GPS y magnetometros. Se emplean diversos metodos para integrar todas o parte de 
estas medidas: 

1.1. - Filtro de Kalman Lineal: Es el de mayor simplicidad y menor coste 
computacional y, por tanto, muy atractivo para aplicaciones de bajo coste. El 
inconveniente es que solo es aplicable a sistemas dinamicos lineales o linealizados. 
Por tanto, solamente en casos concretos podra utilizarse. 

1.2. - Filtro de Kalman Complementario: Se emplean los algoritmos INS para 
integrar las medidas de una IMU, que puede ser de bajo coste. Las entradas al INS se 
corrigen con las salidas de un filtro de Kalman lineal constituido por un modelo de error 
del INS y de las medidas y alimentado por el error entre la posicion y velocidad 
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estimadas por el INS y las medidas por el resto de sensores. El problema es que la 
linealizacion quexonllevarel modeler de~error im plica que no se garantice convergencia 
global y actualizaciones espureas podnan llevar a la divergencia del sistema; ademas, 
como todos los filtros de Kalman, su diseno implica el conocimiento de la estadfstica, 
5 tanto del ruido de medida como el del proceso y que estos sean blancos, gausianos y 
no correlacionados, lo que en el caso de sensores de bajo coste se da con poca 
frecuencia. Su coste computacional oscila entre moderado y alto dependiendo de la 
dimension de los vectores de estado y medidas. 

1.3.- Filtro de Kalman Extendldo: Probablemente es el mas utilizado al ser mas 

10 preciso que el filtro Kalman estandar. Puede estimar la dinamica del vehfeulo que, en 
general, no es lineal, ya que admite terminos no lineales tanto en el modelo como en 
las medidas. Tiene un riesgo mayor de divergencia que el Kalman estandar, pues las 
ecuaciones de covarianza se basan en el sistema linealizado y no en el real no lineal, 
Se ha probado su estabilidad local asintotica, pero no se puede garantizar su 

15 estabilidad global. Ademas de compartir con el resto de los filtros Kalman la necesidad 
de conocer las estadisticas de ruido y medida, su coste computacional es alto. 
2.- Metodo Estatico 6 "Single Frame": donde a diferencia de los metodos de 
filtrado anteriores, se realiza una estimacion estatica, consistente en obtener la 
orientacion a partir de un conjunto de medidas de vectores unitarios tanto en ejes 

20 cuerpo como en ejes referenda. Casi todos los algoritmos (Davenport's, QUEST, 
FOAM...) se basan en resolver el problema de Wahba, que consiste en encontrar una 
matriz ortogonal, con determinante +1, que minimice una funcion de coste compuesta 
por la suma ponderada de los cuadrados del error entre los vectores unitarios en ejes 
cuerpo y el resultado de transformar los vectores en ejes referencia por la matriz que 

25 se busca. Suele aplicarse en sistemas espaciales, donde se obtienen los vectores 
unitarios por apuntamiento al Sol y otras estrellas. 

Hay algunos ejemplos de aplicacion a una estimacion dinamica, pero es a base 
de su integracion en un filtro de Kalman, lineal o complementario, con las desventajas 
enunciadas anteriormente. 

30 Descripcion de la invencion 

La invencion se refiere a un sistema de acuerdo con la reivindicacion 1 y a un 
metodo segun la reivindicacion 7. Realizaciones preferidas del sistema y del metodo 
se definen en las reivindicaciones dependientes. 

Dadas las carencias de la tecnica anterior que pueden resumirse en: 

35 • falta de garantfa de convergencia global; 
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• necesidad de conocer la estadfstica de ruido y proceso; 

• necesidad de que los ruidos de medida y proceso sean blancos, gausianos y 
no correlacionados; 

• coste computacional de moderado a alto; 

5 el objetivo de la presente invencion consiste en suministrar un sistema de fusion de 
sensores de acuerdo con la reivindicacion 1 y un metodo segun la reivindicacion 7 
que: 

• es convergente globalmente en sentido ISS (Input to State Stability); 

• no requiere modelos estadfsticos complejos; y 
10 • presenta un coste computacional bajo. 

Para conseguir los objetivos anteriores hay que resolver el problema de 
estabilizar la integracion de las ecuaciones cinematicas de forma global y sin que 
computacionalmente sea costoso. Para lograrlo, en vez de utilizar metodos de 
estimation estadistica cbmo~en "el "filtrd WKalmari, la presente invencion plantea el 

15 problema como uno de control no lineal y lo resuelve haciendo uso de la teoria de 
estabilidad de Lyapunov, encontrando una ley de control que proporciona las 
correcciones u m que hay que aplicar a la velocidad angular para estabilizar la 
integracion de las ecuaciones cinematicas de forma global asintotica. 

El planteamiento del problema de control no lineal y su solucion ha sido 

20 desarrollado de la siguiente forma: 

a.- Planteamiento: Sean g b9 e b dos vectores unitarios constantes expresados en unos 
ejes fijos al vehfculo (ejes cuerpo), y g n e t los mismos vectores expresados en ejes 
locales (ejes tierra). 

Si B es la matriz de cosenos directores, se tiene: 

25 gb=Bg* 

e b =Be t 

Integrando las ecuaciones cinematicas con la velocidad angular medida, & b , 

obtendriamos una matriz de cosenos directores B , que sabemos erronea por las 
imprecisiones de los sensores. Por tanto, aplicaremos una correccion u m a la 
velocidad angular medida. Llamando & b = a> b + u (0 a la velocidad angular corregida, e 

30 integrandola obtendremos la matriz de cosenos directores B . A partir de esta matriz 
de cosenos directores pueden obtenerse unos vectores estimados g b9 e b : 
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e b = Be, 

Se trata ahora de determinar la correccion u w para que se anule el error 

Sb~ Sb~ 8 b 



— r\ 



[1-1] 



Este es un problems de control no lineal que se va a resolver utilizando una 
extension del concepto de funcion de Lyapunov denominado Funcion de Control de 
Lyapunov (Clf) [refl] en los siguientes pasos: 

1. Formamos el sistema 



z = f(z,u G) ),z = 



L*bA 



,/(0,0) = 0 [1.2] 



— 2. -Encontraremos-una-Glf para-el sistema-anterior, es-deGir-j una funcion continua 
derivable, definida positiva y no acotada radialmente J : R 6 — > R + tal que: 



inf{^(z)/(z ?M jj<a vz*o ti-3] 

i^eR lOZ J 



15 3. Encontraremos una ley de control u & = a(z) tal que si W(z) es una funcion 

definida positiva, se cumpla: 

^(z)f(zMz))<-W(z) [1.4] 
oz 

20 4. El Teorema de LaSalle-Yoshizawa [refl] nos garantiza que el sistema [1.2] con 

u & - a(z) es global, uniforme y asintoticamente estable (GUAS) para el punto 

de equilibrio z=0. 
b.- Resolucion: 

1 . Derivando [1.1] obtenemos el sistema: 

25 [15] 

2. Probaremos la siguiente Clf: 

J{z) = \[g b -gb + Zb-Z b } [1-6] 
cuya derivada segun la solucion de [1.5] es: 
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— \z)f(z>uj = 

oz 

Teniendo en cuenta las propiedades del producto vectorial queda: 
g-(z) f(z, u b ) = -«,. • (g b xg b +e b xe b ) [1.8] 

5 3. Tomamos la siguiente ley de control: 

u< 0 =c(g b xg b + e b xe b ) [1] 
donde a es un escalar positivo. 

Llamando f = g b x g b + e b x e b y substituyendo en [1 .8] se tiene: 

^(^)7(^^)=-cr(f.f) = -a-|f| 2 [1.10] 

10 4. Puesto que r 2 es una funcion definida positiva, se cumple [1.4] y por el 

teorema de LaSalle-Yoshizawa [refl] el sistema [1.5] con u a =dT es global, 

uniforme y asintoticamente estable (GUAS) para el punto de equilibrio 2=0. 
c- Robustez: La Ley de Control anterior cumple con los objetivos propuestos pero 
solo si tenemos medidas perfectas de los vectores g b ,e b , lo que en la practica la 
15 convertiria en inutil. Sin embargo, como se demostrara seguidamente, no es asi y esta 
ley de control sigue siendo valida con medidas erroneas, siempre que esten acotadas, 
como ocurre en la practica. 

Supongamos que las medidas g b ,e b contienen errores aditivos. El vector 
verdadero sera el medido mas los errores de medida: 

20 f = 5 + 5 [1.11] 

e = e + e 

Sustituyendo en la definicion de f se tiene: 

f = (ft+ft)xg A +(e A +g A )xg 4 

= [(ft x ft) + («* x «*)] 
+ [(g b xg b ) + (e b xe b )'] 

= f + f 



[1.12] 



Como unicamente conocemos el valor medido de r, solo podremos utilizar 
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fen las leyes de control, y la ecuacion [1.10] se modifica para sen 

^(z)f(z 9 u„) = -a((f-f)*r) 

oz [1.13] 

= -Cr\ff f) 



con lo que ya no se puede garantizar que el sistema [1.5] con u„ =df sea global, 

uniforme y asintoticamente estable (GUAS) para el punto de equilibrio z=0. Sin 
embargo, demostraremos que es ISS (input to state stable) [ref2] [ref3] [refl], es decir, 
que se cumple la propiedad: 

\z(t)\ < fi(\z(G)\ 9 t) + r(sup |f(r)|) [1.14] 

0&TSI 

donde J3 es una funcion de clase KL y y es una funcion de clase K [refl]. 
Utilizando la inigualdad de Young se tiene: 



.1 



r-r"ST"r+-(r-T) _ 



n?i5] 



y sustituyendo en [1.13]: 

^ -0.75o-|rf +cr|f f 



[1.16] 



15 



20 



que prueba que ^-(z)f(z,u i) )<0, Vz *0 , siempre que |rf >^||f|f. 

oz 3 00 

Como J(z) y r 2 (z) son definidas positivas y no estan acotadas radialmente y 

J(z) es lisa, existen funciones y l ,y 2 ,y 3 de clase K m tales que: 

ri (\z\)<J(z)<y 2 (\z\) 
y 3 (\z\)<r 2 (z) 

Por tanto, 



[1.17] 



*co|>^(||ft 



J<0 



Lo que implica que si: 



entonces: 



[1.18] 



> # 
i » « * 



.1 5 a t » 
, » • 

'.' 3 9 



que a su vez implica que 



H$*r?*r2*r?(±\Fty [1-19] 

Si, por otra parte 
resulta que 

|z(0|<yr lo / 2 (K0)|) [1.20] 

Por tanto, teniendo en cuenta [1.19] y [1.20] se demuestra que z{t) esta 
acotada globalmente: 

ML * ™*{* 1 °r 2 •ys*(fpt).y^ M)} 

y converge al conjunto residual: 

t5 = |^ : |^ < o r2 o ||r£)[ j [1.21] 

Ademas, por el Teorema C.2 [refl] (formulado en [ref2]) el sistema [1.5] con la 
Ley de Control = of es ISS con respecto a los errores de los sensores f . 

Asf, la presente invencion se refiere a un sistema para estimar posicion, 
velocidad y orientacion de un vehfculo, como puede ser una aeronave no tripulada, 
que incluye: 

medios para determinar las componentes, segun ejes cuerpo del vehfculo, de 
dos vectores unitarios constantes no colineales g b ,e b \ 

medios para determinar las componentes, segun ejes tierra, de dichos vectores 
unitarios constantes no colineales g t9 e t ; y 

medios para determinar las tres componentes de la velocidad angular 3 b del 
vehfculo en ejes cuerpo. 

El sistema de fusion de sensores de la invencion incluye ademas: 

medios para corregir la velocidad angular 3 b con una correccion u m y obtener 
una velocidad angular corregida & b = 5> b + u w ; 

un modulo de integracion de las ecuaciones cinematicas del vehfculo, que 
recibe como entrada la velocidad angular corregida &> b y proporciona la matriz de 

transformacion de ejes tierra a ejes cuerpo del vehfculo B y la orientacion del vehfculo 
en forma de angulos de Euler 6 ; 
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un modulo de sintesis de las componentes en ejes cuerpo de los dos vectores 
unitarios constantes no colineales para proporcionar una estimacion de dichos 
vectores no colineales en ejes cuerpo g b ,e b , donde dicha estimacion se calcula segun: 

g b =Sg, 
e b = Be, 

5 . un modulo de control, que implementa una ley de control para calcular dicha 
correccion u w , donde dicha ley de control es: 

u a =<r(g b xg b + e b xe b ) [1] 

siendo cr un escalar positive 

de forma que al aplicar esta correcci6n u a a la velocidad angular medida 5> b y utilizar 
10 la velocidad angular resultante cb b = 3 b +u a como entrada al modulo de integracion de 
las ecuaciones cinematicas, estas -son.- estables en sentido ISS y el error en la 
estimacion de la matriz de cosenos directores B y de los angulos de Euler <f> se 
mantiene acotado. 

Preferiblemente dichos vectores unitarios no colineales g,e son gravedad local 
15 gy proyeccion del campo magnetico sobre el piano perpendicular a la gravedad e . 

Preferiblemente, los medios para determinar las componentes, segun ejes 
cuerpo del vehfeulo, de los dos vectores unitarios constantes no colineales g b ,e b 

incluyen: 

una Unidad de Medida Inercial (IMU), que incluye un conjunto de al menos tres 
20 giroscopos y al menos tres acelerometros situados segun los ejes cuerpo del vehiculo; 
. un magnetometro con capacidad para medir el campo magnetico terrestre 
segun los ejes cuerpo del vehiculo; 

sensores de presion estatica y diferencial; 
dos veletas para medir los angulos de ataque y resbalamiento; 
25 - un modulo de adquisicion y proceso de la velocidad angular, configurado para 
adquirir la velocidad angular 3 b (t) medida por los giroscopos y retrasarla para obtener 

5> b (t-T)\ 

un modulo de adquisicion y proceso de datos configurado para adquirir la 
fuerza especlfica a b {t) medida por los acelerometros, la presion estatica p s (t) , la 
30 presion diferencial p d (t) , el angulo de ataque a(t) , el angulo de resbalamiento 
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y el valor del campo magnetico terrestre m b (t) medido en el magnetometro, 
retrasarlos y procesarlos para calcular la velocidad verdadera respecto al aire 0(t-r) , 
la velocidad aerodinamica en ejes cuerpo v b (t-z) segun: 

v cos a cos jS 
v b = usin p 
^ sin 5 cos p 

5 calcular la derivada numerica de la velocidad aerodinamica en ejes cuerpo 9 b (t - r) , 
calcular la gravedad local en ejes cuerpo g b segun: 

g b (t ~ T) = %(t ~T) + 3 b (t-T)x V b (t - T )-a b (t- T) 

y calcular la proyeccion del campo magnetico terrestre sobre el piano horizontal 
perpendicular a la gravedad local e{t-v) segun: 

Los medics para conocer las componentes, segun ejes tierra, de los dos 
vectores unitarios constantes no colineales g n e, pueden incluir: 

un receptor GPS (Global Positioning System), 
y en dicho modulo de adquisicion y proceso de datos (17) se adquieren los dates 
proporcionados por el GPS, se procesan y se utilizan para calcular las componentes, 
segun los ejes tierra, de los dos vectores unitarios constantes no colineales jr e 

El sistema puede preferiblemente incluir un filtro de Savitzky-Golay en el que se 
calcula v b , derivada numerica dev.. 

b 

De acuerdo con un aspecto adicional de la presente invencion, se proporciona 
un sistema de fusion de sensores para estimar posicion y velocidad de un vehiculo, en 
particular una aeronave no tripulada, que preferiblemente ademas incluye: 

medios de adquisicion de datos de un grupo de sensores situados en el 
vehiculo, como podria ser un receptor GPS (Global Positioning System) y un sensor 
de presion estatica, que proporcionan posicion y velocidad en unos ejes fijos a tierra 
25 P„V,- 

en caso de que los medios para conocer las componentes, segun los ejes 
cuerpo del vehiculo, de los vectores unitarios constantes mencionados anteriormente 
no incluyesen medios de adquisicion de datos de un conjunto de tres acelerometros 
.tuados segun los ejes cuerpo del vehiculo, estos se incluiran para proporcionar la 



fuerza especifica a b en ejes cuerpo; 

un modulo de navegacion, donde a partir de la fuerza especifica a b y de la 

matriz de cosenos directores B se integran las ecuaciones de navegacion del 
vehfculo para obtener posicion y velocidad calculadas en ejes locales y se corrigen en 
un filtro de Kalman, que puede ser estatico, para obtener posicion y velocidad 
estimadas en ejes locales. 

En el caso de una aeronave no tripulada todos estos requerimientos de medios 
para conocer diversos parametros o de adquisicion de datos de sensores pueden 
cumplirse con los sensores de los que habitualmente disponen este tipo de aeronaves, 
como se vera en los siguientes apartados, y por tanto, la cantidad de sensores 
necesarios no supone una desventaja de la presente invencion, sino mas bien al 
contrario una ventaja en cuanto se trata de extraer la maxima information de los 
. sensores _de que„se dispone ? „ 

Es otro objetivo de la presente invencion proporcionar un metodo de fusion de 
sensores para estimar posicion, velocidad y orientation de un vehfculo, que 
comprende: 

calcular las componentes, segtin ejes cuerpo del vehfculo, de dos vectores 
unitarios constantes no colineales g b ,e b a partir de medidas de sensores situados en 

el vehfculo segun los ejes cuerpo de este; 

calcular las componentes, segtin ejes tierra, de dichos vectores unitarios 
constantes no colineales g t ,e n a partir de medidas de sensores situados en el 
vehfculo que proporcionan posicion en ejes locales; 

medir las tres componentes de velocidad angular S> b del vehfculo en ejes 

cuerpo; 

corregir la velocidad angular 3 b con una correction u w y obtener una 
velocidad angular corregida d> b =3 b ^-u Q) ) 

integrar las ecuaciones cinematicas del vehfculo, en funcion de la velocidad 
angular corregida & b y proporcionar la matriz de transformation B de ejes tierra a 

ejes cuerpo del vehfculo y la orientacion del vehfculo en forma de angulos de Euler 6 ; 

calcular una estimacion de las componentes en ejes cuerpo de los dos vectores 
unitarios constantes no colineales g b9 e b , donde dicha estimacion se calcula segun: 



e b =Be, 

obtener la correccion u„ mediante la ley de control: 
siendo a un escalar positivo, 

de forma que al aplicar esta correccion u w a la velocidad angular medida 3 b y utilizar 

la velocidad angular resultante & b =a> b + Uc3 como entrada al modulo de integracion de 

las ecuaciones cinematicas, estas son estables en sentido ISS y el error en la 

estimation de la matriz de cosenos directores B y de los angulos de Euler 6 se 
mantiene acotado. 

Referencias: 

[refl] Miroslav Krstic, loannis Kanellakopoulos, Petar Kokotovic, "Nonlinear and 

adaptive Control Design", Willey,1995. 
[ref2] E. D. Sontag, Smooth stabilization implies co-prime factorization, IEEE 

Transactions on Automatic Control, AC-34 (1989) 
[ref3] E. D. Sontag, Yuang Wang, On Characterizations of the Input-to-State Stability 

Property. 

[ref4] M. Mandea et al., International Geomagnetic Reference Field - Epoch 2000 
Revision Of The IGRF for 2000 - 2005, http://www.nqdc.noaa.gov/IAGA/wa8/ 
iqrf.html . 26 May 2000. 
[ref5] Brian LStevens,Frank L.Lewis, "Aircraft Control and Simulation", Willey 1992 
[ref6] Hans W. Schussler, Peter Stefen, "Some Advanced Topics in Filter Design", in 
Advanced Topics in Signal Processing Prentice Hill 1988. 

Breve description de los dibuios 
A continuacion se pasa a describir de manera muy breve una serie de dibujos 
que ayudan a comprender mejor la invencion y que se relacionan expresamente con 
una realizacion de dicha invencion que se presenta como un ejemplo no limitativo de 
esta. 

La figura 1 muestra en un diagrama de bloques el sistema de la invencion en 
un sistema de navegacion, guiado y control en una aeronave no tripuladas (UAV). 

La figura 2 se muestra el esquema de implementacion del sistema de la 
invencion como elemento integrante de un sistema de control de vuelo. 
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La figura 3 presenta un diagrama de bloques general de los elementos de los 
que consta el sistema de la invencion. 

La figura 4 muestra en diagrama de bloques una vista detallada del modulo de 
resolucion de ecuaciones cinematicas. 
5 La figura 5 muestra un diagrama de bloques detallado de los modulos de 

adquisicion y proceso de la velocidad angular y de adquisicion y proceso de datos de 
sensores. 

La figura 6 muestra en diagrama de bloques detallado del modulo de 
navegacion. 

10 Descripcion de una realizacion pre ferida de la invencion 

Como ejemplo del mejor modo de llevar a la practica la invencion se presenta 
la realizaci6n de una unidad de fusi6n de sensores para un sistema de control de vuelo 
de una aeronave no tripulada (UAV) de corto-medio alcance y baja velocidad, sin que 
-esto suponga que no sean posibles otras implementaciones. 

15 En la figura 1 se presenta el sistema de control de vuelo en una aeronave no 

tripulada 10, constituido por sensores 2, el sistema 1 de la invencion, un sistema de 
control y guiado 3 y actuadores 4. El sistema 1 de la invencion suministra informacion 
de actitud, velocidad y posicion al sistema de control y guiado 3, que a su vez 
proporciona la correspondiente informaci6n a los actuadores 4 de la aeronave. Son 

20 entradas del sistema de la invencion la informacion proporcionada por un conjunto de 
sensores, que en esta figura 1 se representan conjuntamente en una unica caja como 
sensores 2. 

En la figura 2 se detallan dichos sensores, que son: 

una Unidad de Medida Inercial (IMU) 21, que incluye un conjunto de tres 
25 giroscopos 211 y tres acelerometros 212 situados segun los ejes cuerpo del vehfculo 
(en este caso se trata de una IMU de estado solido y bajo coste, por ejemplo, 
Crossbow IMU400); 

un magnetometro 22 con capacidad para medir el campo magnetico terrestre 
segun los tres ejes cuerpo del vehiculo; 
30 - un tubo de Pitot 23, que incluye sensores de presion estatica 231 y diferencial 

232; 

dos veletas 24 dotadas de potenciometros para medir los angulos de ataque 

241 y resbalamiento 242; 

un receptor GPS 25 (Global Positioning System) con una unica antena. 
35 La mejor realizacion contemplada del sistema de la invencion es un sistema 
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basado en microprocesador, compuesto por tarjetas PC-104. Como se presenta en la 
figura 2 el sistema consta de una tarjeta de adquisicion analogico-digital 26 (por 
ejemplo, DM6420 de RTD embedded technologies) para adquirir datos del 
magnetometro 22, las veletas 24 y el Pitot 23, una tarjeta de puertos serie RS-232 29 
5 (por ejemplo, CM310 de RTD embedded technologies) para adquirir datos de la IMU 
21 y del GPS 25, una tarjeta de CPU 28 (por ejemplo, CMV6486 de RTD embedded 
technologies) que contiene al microprocesador y la memoria donde residen en forma 
de programa ejecutable los algoritmos de los modulos 11, 12, 13, 14, 15, 16 y 17 
(figura 3). El sistema dispone de un bus PC-104 20, que comunica a las diferentes 
10 tarjetas y una tarjeta de alimentacion electrica 27 que tiene como entrada una 
alimentacion estandar de avion de 28 voltios y genera y distribuye a traves de bus los 
+5F,±12Fque necesitan las tarjetas. 

En esta realizacion, se toman como vectores unitarios constantes no colineales 
las direcciones de la gravedad local g y de la proyeccion del campo magnetico 
15 terrestre sobre el piano horizontal perpendicular a la gravedad local e, que se 
consideran constantes. Las componentes, tanto en ejes cuerpo como en ejes'tierra, de 
estos vectores se calculan, a partir de medidas de sensores mencionados 
anteriormente, en el modulo de adquisicion y proceso de datos de sensores 17. 

Asf, para calcular g b , la fuerza especifica a„(t) medida por los acelerometros 
212 se adquiere en el modulo 17 y se retrasa un tiempo r , correspondiente a cinco 
periodos de muestreo, en el FIFO 170 para obtener a b {t-r) y suministrarla al m6dulo 
178 . De forma similar, la velocidad angular 3 b (t) medida por los giroscopos 211 se 
adquiere en el modulo de adquisicion y proceso de la velocidad angular 1 1 y se retrasa 
en el FIFO 111 para obtener 3 b (t-r) que se suministra al modulo 178. La presion 
estatica p s medida en el sensor 231 y la presion diferencial p d medida en el sensor 
232 se adquieren en el modulo 17 y se procesan en el modulo de calculo de altitud y 
velocidad 171 para obtener D la velocidad verdadera respecto al aire (TAS, true air 
speed) y la altitud h por un procedimiento que es conocido para un experto en la 
materia. Como en este procedimiento se requiere conocer la temperatura ambiente en 
el exterior T Q , esta se suministra inicialmente, antes del vuelo, por ejemplo en un 
fichero de inicializacion. La velocidad verdadera respecto al aire 0 se suministra al 
modulo 176 junto con el angulo de ataque a , adquirido del sensor 241, y el angulo de 
resbalamiento , adquirido del sensor 242, y en dicho modulo 176 se calcula el 
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vector velocidad respecto al aire en ejes cuerpo v b (t) de la siguiente forma: 

0 cos a cos $ 



Csin fi 
v sin a cos fi 



Esta velocidad se retrasa un tiempo t para obtener v b {t-r) que se suministra 
al modulo 178. La velocidad v b (t) se deriva numericamente utilizando un filtro de 

5 Savitzky-Golay 179 [ref 6] de orden cuatro con una ventana de 11 muestras, 
calculandose la derivada para la muestra con indice 5, es decir, la derivada esta 
retrasada r, cinco periodos de muestreo. Esta derivada v b (t-r) se suministra al 
modulo 178 donde, finalmente, se calculan las componentes en ejes cuerpo del vector 
gravedad local g b (t-r) segun: 

10" " " g'tl* -*T=%(t ~ r ) + -*)*v b (t-T)-a b (t-T) 

que corresponden a las ecuaciones traslacionales del movimiento del vehiculo con las 
hipotesis de "tierra plana" , gravedad constante, acelerometro situado en el centra de 
gravedad del vehiculo y velocidad del viento constante. A la vista de estas ecuaciones 
se justifies la razon de retrasar las diferentes variables. Obtener la derivada numerica 

15 de la velocidad, que es una variable que contiene ruido, no es tarea sencilla; una de 
las mejores soluciones es el filtro de Savitzky-Golay, pero este es un filtro no causal, 
por lo que hay que retrasar el instante en que se calcula la derivada. 

Para calcular e„, en primer lugar se adquiere, en el modulo 17, el valor del 
campo magnetico terrestre m b (t) medido en el magnetometro 22 y se retrasa un 

20 tiempo r en el FIFO 174 para obtenerse m b (t-r) y suministrarse al modulo 175. En 
este modulo 175, al que tambien se ha proporcionado g b (t-T) , se realiza el calculo: 

e b (t -t) = m„(t -z)-m b (t-T)- 

El calculo de las componentes en ejes tierra de los vectores unitarios en la 
direccion de la gravedad local y la proyeccion del campo magnetico terrestre sobre el 
25 piano horizontal perpendicular a la gravedad local g„e, se realiza en el modulo 173. 
En este modulo 173 se implementa un modelo del International Geomagnetic 
Reference Field (IGRF) [ref 4] que proporciona las componentes del campo magnetico 
terrestre en ejes tierra in, dada la posicion del vehiculo ^ . De la misma forma un 
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modelo de gravedad local proporciona g, a partir de la posicion del vehfculo P,. Para 
obtener esta posicion, en el modulo 17 se adquieren las medidas del GPS 25, se 
procesan en el modulo 172 para convertirlas del sistema de referenda del GPS a ejes 
tierra y la posicion P t asf obtenida se suministra al modulo 173. 

Los tres giroscopos 211 situados segun los ejes cuerpo de la aeronave miden 
las tres componentes de la velocidad angular 3 b en estos ejes. Esta es adquirida en 
un modulo de adquisicion y proceso de la velocidad angular 11, donde en un FIFO 111 
se retrasa un tiempo t correspondiente a cinco periodos de muestreo. El modulo 11 
suministra velocidad angular actual 3 b (t) al modulo de resolucion de ecuaciones 
cinematicas 13, y velocidad angular retrasada d> b (t-r) al modulo 12, donde le suma 
la correccion u m (t-r) calculada en el m6dulo de control 14 para obtener una 
- velocidad angular-corregida-^ - r) = S b (t - t) + r)., que tambien se suministra 
al modulo 13. 

En el modulo de resolucion de ecuaciones cinematicas 13 se utiliza el 
quaternion q T =[q 0 q, q 2 q 3 ] como medio de representacion de la actitud. 
Inicialmente el quaternion se calcula en el modulo 134 a partir de los angulos de Euler 
= [</> 0 v] iniciales que se suministran externamente, segun: 



?o Co) 



V2) 



cos 



( e ^ 

ZSL 

V2) 



sin 



{2 



[2.1] 



q M = sin[A )cos^]cos[^)-cos[^)sin[| 

Las ecuaciones cinematicas del movimiento del vehfculo expresadas en 
funcion del quaternion son: 



4o 




0 




CO, 


0) 2 ' 


~9o~ 




_ 1 


-co 0 


0 




CO, 






~ 2 


-co, 


C0 2 
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-co 0 
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-?3_ 




—co 2 


-co, 


co 0 


0 


-?3_ 



[2.2] 



que abreviadamente representaremos como: ?(0 = ¥(*> 6 (0)<?(0 • Integraremos estas 
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ecuaciones en dos fases: 

*(f)«Jf*(fW+}*tfW. 

en una primera fase utilizaremos la velocidad angular corregida & en las ecuaciones 
diferenciales [2.2] y las integraremos en el modulo 137 utilizando, por ejemplo, el 
metodo de McKern [ref5] desde el tiempo inicial hasta t-t. ya que solamente se 
dispone de y no de cb b {t) en cada instante t. De esta forma obtenemos 

q(t-r)y como se demostro al desarrollar la ley de control sabemos que el error 
~ {t , T) = qit _ T) _ _ r) esta acotado. La segunda fase de la integraci6n se realiza 
en el modulo 132 donde se Integra la derivada del quaternion, calculada en el modulo 
131 utilizando la velocidad angular 3 b {t) , desde el instante t-r hasta t, y.utilizando 
como valor inicial en r-rel quaternion estimado en el modulo 137: q(t-r) . Dado 
que esta segunda fase de la integracion se desarrolla durante un tiempo muy pequeno 
r , corrispondiente a cinco muestras, el error cometido por utilizar 3 b (t) e vez de 
4(0 as despreciable y como resultado se obtiene *(') que se distribuye al modulo 
138 donde se calcula la matriz de transforation de ejes tierra a ejes cuerpo B(t) 
segtin: 

2(m+m) 2(«ft + aa) 

utilizando el quaternion q(t). De la misma forma, en el modulo 138 se calcula B{t-r) 
utilizando g(t-r) proporcionado por el modulo 137. 

Conocidos £(*-r) proporcionado por el modulo 137 y e t (t-T%g,(t-r) 
proporcionados por el 17, en el modulo 15 se sintetiza e b {t-r\g b (t-r) segun: 

2 b (t-r) = W-T)e t 

y en el modulo 14 se calcula la correccion u.« -t) anterior mediante la ley de control: 
^(f-r) = cj(&(r-r)x&(/-r)+^-T)x^-T)) 



B= 
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donde a es una ganancia escalar constante positiva, que se ha seleccionado en este 
caso a un valor de 0.1. 

Como se muestra en la figura 6, el modulo de navegacion 16 consiste en tres 
filtros Kalman estacionarios uno para cada uno de los canales x,y,z, 161, 162, 163 
respectivamente, y se calcula en el modulo 164 la aceleracion en ejes tierra a partir de 

la fuerza especffica a b {t) y gravedad g t proporcionadas por el modulo 17 y B{t) 

proporcionada por el modulo 13: 

a t ^B T a h +g t 

y se suministra cada componente de a t como senal de control a cada uno de los 
filtros. Suponemos que a t contiene ruido exponencialmente correlacionado, pudiendo 
describirse como: 



a t = a t + r 



r = -yr + e r 

donde s r es ruido bianco de media cero y y es diagonal. Por tanto, el vector de 

estado basico consistente en p t V t posicion y velocidad se aumenta con r, 

correspondiente al "shaping filter" anterior, para tener en cuenta la correlacion en la 
aceleracion. 

Consideramos que la velocidad de la masa de aire respecto a tierra se 
compone de una constante V w , la velocidad del viento, y un ruido correlacionado w g , 
que representa la turbulencia atmosferica y puede representarse por: 
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^=[^0 Mi] 



donde S s es ruido bianco con media cero, y las constantes /L,ju se determinan a partir 

de la densidad espectral especificada en [Mil. Spec. 1797]. 

Para poder identificar la velocidad del viento en ejes tierra, incorporaremos el 
"shaping filter" anterior anadiendo tres componentes mas al vector de estado de cada 
uno de los filtros: V^s^s^ . Finalmente el vector de estado queda: 

17" 



-19- 

Las ecuaciones de estado son: 

r = —yr + s r 

5 0 =S, 

Como medidas, ademas de las del GPS p n V t > incorporamos la velocidad 

aerodinamica en ejes tierra: 

5 v ( =B T v b 

que se calcula en e! modulo 165. 

Tambien incorporamos la medida de la altura barometrica, pero solo en el filtro 
correspondiente al canal z: 

h-h 0 =-p t2 +?j h 
10 Las ecuaciones de medida quedan: 

donde tj es ruido bianco con media cero. 
Los filtros Kalman quedan: 

x. - Aft + Ba ti + K t [y t - Cft] 
1=0,1,2 

15 donde: 
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2 " 0 1 0 -M 02 -Jll, 2 -1 

-1 0 0 0 0 0 

y las ganancias K, se obtienen resolviendo la correspondiente ecuacion algebraica de 

Riccati dadas las matrices de covarianza del ruido de proceso Q. y medida R t : 

z = 0,1,2 
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REIVINDICACIONES 

1. Sistema para estimar posicion, velocidad y orientacion de un vehfculo (10), que 
comprende: 

medios para determinar las componentes, segtin ejes cuerpo del vehfculo, de 
5 dos vectores unitarios constantes no colineales g bi e b \ 

medios para determinar las componentes, segun ejes tierra, de dichos vectores 
unitarios constantes no colineales g n e ( ; 

medios para determinar las tres componentes de velocidad angular 3 b del 

vehfculo en ejes cuerpo; 
10 caracterizado porque el sistema comprende: 

medios para corregir dicha velocidad angular 3 b con una correccion u & y 

obtener una velocidad angular corregida <& b = cd b +u 0 \ 

un modulo de integracion de las ecuaciones cinematicas (13) del vehfculo, que 
recibe como entrada la velocidad angular corregida co b y proporciona la matriz de 

15 transformation B de ejes tierra a ejes cuerpo del vehfculo y la orientacion del vehfculo 

en forma de angulos de Euler 6 ; 

un modulo de stntesis (15) de las componentes en ejes cuerpo de los dos 
vectores unitarios constantes no colineales para proporcionar una estimacion de 
dichos vectores no colineales en ejes cuerpo g b ,e b , donde dicha estimacion se calcula 
20 segun: 

g b =Bg t 
e b = Be t 

un modulo de control (14), que implementa una ley de control para calcular 
dicha correccion u a , donde dicha ley de control es: 

u (0 ^<J{g b ^g h +e b ^e b ) [1] 
25 siendo a un escalar positivo, 

de forma que al aplicar esta correccion u 0 a la velocidad angular medida co b y utilizar 

la velocidad angular resultante d> b ~& b - } ru 0} como entrada al modulo de integracion de 
las ecuaciones cinematicas, estas son estables en sentido ISS y el error en la 
estimacion de la matriz de cosenos directores B y de los angulos de Euler 6 se 
30 mantiene acotado. 
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2. Sistema segiin la reivindicacion 1, caracterizado porque dichos vectores 
unitarios no colineales g,e son gravedad local g y proyeccion del campo magnetico 
sobre el piano perpendicular a la gravedad e . 

3. Sistema segun la reivindicacion 2, caracterizado porque los medios para 
5 determinar las componentes, segtin ejes cuerpo del vehfculo, de los dos vectores 

unitarios constantes no colineales g b9 e b incluyen: 

una Unidad de Medida Inercial (IMU) (21), que incluye un conjunto de al menos 
tres giroscopos (211) y al menos tres acelerometros (212) situados segOn los ejes 
cuerpo del vehfculo; 

10 un magnetometro (22) con capacidad para medir el campo magnetico terrestre 

segun los ejes cuerpo de! vehfculo; 

sensores de presion estatica (231) y diferencial (232); 

dos veletas (24) para medir los angulos de ataque (241) y resbalamiento (242); 
un modulo de adquisicion y proceso (11) de la velocidad angular, configurado 
15 para adquirir la velocidad angular co b (t) y retrasarla para obtener S b (t-r) ; 

un modulo de adquisicion y proceso de datos (17) configurado para adquirir la 
fuerza especifica a b (t) medida por los acelerometros (212), la presion estatica p s (t) 

medida en el sensor (231), la presion diferencial p d (t) medida en el sensor (232), el 

angulo de ataque a(f) medido en el sensor (241), el angulo de resbalamiento fi(t) 
20 medido en el sensor (242) y el valor del campo magnetico terrestre fh b {t) medido en el 
magnetometro (22), retrasarlos y procesarlos para calcular la velocidad verdadera 
respecto al aire D(t-r), fa velocidad aerodinamica en ejes cuerpo v b {t-r) segun: 

v cos a cos J3 

vsin fi 
v sin a cos jB 

!a derivada numerica de la velocidad aerodinamica en ejes cuerpo v h {t-z) , 
25 la gravedad local en ejes cuerpo g b segun: 

g b (t-T) = v b (t-T) + 3 b (t-r)xv b (t-T)-a b (t-T) 

y la proyeccion del campo magnetico terrestre sobre el piano horizontal perpendicular 
a la gravedad local e(t-r) segun: 
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e b (t - t) = m b (t - r) - m b {t - tr) ■ g*^ _ ^ ; 

4. Sistema segun la reivindicacion 3, caracterizado porque los medios para 
conocer las componentes, segun ejes tierra, de los dos vectores unitarios constantes 
no collneales g n e t incluyen: 

un receptor GPS (25) (Global Positioning System); 
y porque en dicho modulo de adquisicion y proceso de datos (17) se adquieren los 
datos proporcionados por el GPS, se procesan y se utilizan para calcular las 
componentes, segun los ejes tierra, de los dos vectores unitarios constantes no 
colineales g t9 e t . 

5. Sistema segun la reivindicacion 3, caracterizado porque el sistema incluye un 
filtro de Savitzky-Golay (179) en el que se calcula v b , derivada numerica de v b . 

6. Sistema segun la reivindicacion 1, que incluye: 

medios de adquisicion de datos de un grupo de sensores (25, 231) situados en 
el vehfculo, que proporcionan posicion y velocidad segun ejes tierra P n V t \ 

medios de adquisicion de datos de otro grupo de sensores (212) situados en el 
vehfculo, que proporcionan fuerza especlfica a b en ejes cuerpo; 

un modulo de navegacion, donde a partir de la fuerza especffica a b y de la 

matriz de cosenos directores B se integran las ecuaciones de navegacion del 
vehfculo para obtener posicion y velocidad calculadas en ejes locales y se corrigen en 
un filtro de Kalman para obtener posicion y velocidad estimadas en ejes locales. 

7. Metodo para estimar posicion, velocidad y orientacion de un vehfculo (10), que 
comprende: 

calcular las componentes, segun ejes cuerpo del vehfculo, de dos vectores 
unitarios constantes no colineales g b ,e b a partir de medidas de sensores situados en 

el vehfculo segun los ejes cuerpo de este; 

calcular las componentes, segun ejes tierra, de dichos vectores unitarios 
constantes no colineales g n e t , a partir de medidas de sensores situados en el 
vehlculo que proporcionan posicion en ejes locales; 

medir las tres componentes de velocidad angular 5> b del vehiculo en ejes 

cuerpo; 

corregir la velocidad angular co b con una correccion u m y obtener una 
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velocidad angular corregida S b = fi b +u a ; 

integrar las ecuaciones cinematicas del vehfculo, en funcion de la velocidad 
angular corregida & b y proporcionar la matriz de transformacion B de ejes tierra a 
ejes cuerpo del vehfculo y la orientacion del vehfculo en forma de angulos de Euler <f> ; 

calcular una estimacion de las componentes en ejes cuerpo de los dos vectores 
unitarios constantes no colineales g b ,i b , donde dicha estimacion se calcula segun: 



e 



b 



Be, 



obtener la correccion u a mediante la ley de control: 

u o=o-(g b xg b +e b xe b ) [1] 
siendo <x un escalar positive 

de forma que al aplicar esta correccion u a a la velocidad angular medida S b y utilizar 

la velocidad angular /esultante & b = 3 b + u a como entrada al m6dulo de integracion de 

las ecuaciones cinematicas, estas son estables en sentido ISS y el error en la 

estimacion de la matriz de cosenos directores B y de los angulos de Euler 6 se 
mantiene acotado. 

8. Metodo segun la reivindicacion 7, caracterizado porque dichos vectores 
unitarios no colineales g,e son gravedad local g y proyeccion del campo magnetico 
sobre el piano perpendicular a la gravedad e . 

9. Metodo segun la reivindicacion 8, caracterizado porque comprende: 

medir fuerza espeeffica a b (t) , presion estatica p s (t) , presion diferencial p d (t) , 

angulo de ataque a(t) , angulo de resbalamiento fi{fy y valor del campo magnetico 
terrestre m b (t) ; 

calcular la velocidad verdadera respecto al aire u(t) a partir de las medidas de 
presion diferencial p d (t), presion estatica p s (t) y conocida la temperatura exterior en 
el momento inicial T 0 ; 

calcular la velocidad aerodinamica en ejes cuerpo segun: 

v cos a cos (3 

Osin ft 
v sin a cos jB 



v t = 
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retrasar un periodo r la velocidad angular 3 b (t) , la fuerza especifica a b (t) , ei 
campo magnetic© rh b (t) y la velocidad aerodinamica en ejes cuerpo v b (t) ; 

calcular la derivada numerica de la velocidad aerodinamica en ejes cuerpo 

calcular la gravedad local en ejes cuerpo g b segun: 

calcular la proyeccion de) campo magnetico terrestre sobre el piano horizontal 
perpendicular a la gravedad local segdn: 

e b (t-z) = m b (t-z)- m h (t-r). & ( J = ^ , 

10. Metodo segCin la reivindicacion 9, caracterizado porque se mide la posicion en 
ejes tierra P t y a partir. de asta me.dida se. calculan las componentes, segun los ejes 
tierra, de los dos vectores unitarios constantes no colineales g n e t . 

11. Metodo segun cualquiera de las reivindicaciones 9-10, caracterizado porque 
v b , derivada numerica de v b , se calcula en un filtro de Savitzky-Golay (179). 

12. Metodo segun cualquiera de las reivindicaciones 7-1 1 , que incluye: 
medir posicion y velocidad en unos ejes fijos a tierra P n V t ; 

medir fuerza especifica a b en ejes cuerpo; 

integrar las ecuaciones de navegacion del vehiculo en funcion de la fuerza 
especifica a b y de la matriz de cosenos directores B para obtener posicion y 
velocidad calculadas en ejes locales y se corrigen en un filtro de Kalman para obtener 
posicion y velocidad estimadas en ejes locales. 
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